﻿#include "Tutorial.h"
using namespace std;
using namespace ASL;

// 3.1 线性代数运算案例
void Tutorial::Example31(void)
{
    cout << "------ 3.1 线性代数运算案例 ------" << endl
         << endl;

    // 创建指定维度矩阵对象
    Matrix A(5, 5), B, H;

    // 通过函数运算符访问指定行列元素
    A(0, 0) = 0.8147;
    A(0, 1) = 0.0975;
    A(0, 2) = 0.1576;
    A(0, 3) = 0.1419;
    A(0, 4) = 0.6557;
    A(1, 0) = 0.9058;
    A(1, 1) = 0.2785;
    A(1, 2) = 0.9706;
    A(1, 3) = 0.4218;
    A(1, 4) = 0.0357;
    A(2, 0) = 0.1270;
    A(2, 1) = 0.5469;
    A(2, 2) = 0.9572;
    A(2, 3) = 0.9157;
    A(2, 4) = 0.8491;
    A(3, 0) = 0.9134;
    A(3, 1) = 0.9575;
    A(3, 2) = 0.4854;
    A(3, 3) = 0.7922;
    A(3, 4) = 0.9340;
    A(4, 0) = 0.6324;
    A(4, 1) = 0.9649;
    A(4, 2) = 0.8003;
    A(4, 3) = 0.9595;
    A(4, 4) = 0.6787;
    cout << "Matrix A" << endl;
    cout << A << endl;

    // 矩阵基本运算 H = (A+2*A^T)*A*|A|
    cout << "Matrix (A+2*A^T)*A*|A|" << endl;
    H = (A + Matrix::transpose(A) * 2.0) * A * Matrix::determinant(A);
    cout << H << endl;

    // 矩阵求逆运算 H = A^-1
    cout << "Matrix A^-1" << endl;
    H = Matrix::inverse(A);
    cout << H << endl;

    // 矩阵正交分解 A = Q*R
    Matrix Q, R;

    Matrix::householderQR(A, Q, R);

    cout << "Matrix Q" << endl;
    cout << Q << endl;
    cout << "Matrix R" << endl;
    cout << R << endl;

    // 矩阵三角分解 A = L*U
    Matrix L, U;

    Matrix::gaussLU(A, L, U);

    cout << "Matrix L" << endl;
    cout << L << endl;
    cout << "Matrix U" << endl;
    cout << U << endl;

    // 矩阵特征值求解 A*X = Lamda*X
    vector<complex<double>> E;

    E = Matrix::eigenValue(A);

    cout << "Matrix Eigen" << endl;

    for (int n = 0; n < E.size(); n++)
    {
        Basic::display(cout, E[n]);
        cout << endl;
    }

    cout << endl;
}

// 3.2 数值积分求解案例
void Tutorial::Example32(void)
{
    cout << "------ 3.2 数值积分求解案例 ------" << endl
         << endl;

    // 创建积分器对象
    Calculus A;

    // 初始化积分器配置
    // 计算模式 数值积分模式
    // 积分初值 0.0
    // 计算步长 0.005 s
    // 积分方法 欧拉法 ODE-1
    A.initialize(Calculus::ModeType::Integrate, 0.0, 0.005, Calculus::MethodType::Euler);

    // 设置积分限幅
    // 是否启用限幅功能 true/false
    // 设置计算限幅值域 -0.5~+0.5
    A.limit(false);
    A.limit(-0.5, 0.5);

    // 模拟计算时序循环
    for (int n = 0; n < 100; n++)
    {
        double t, u, y;

        // 更新仿真时刻 t(n+1)=t(n)+0.005
        t = n * 0.005;

        // 待积分函数信号 f(x)=sin(2*PI*t)
        u = sin(t * 2.0 * PI);

        // 执行积分器的单步计算函数 返回当前周期计算结果
        y = A.step(u);

        // 访问积分器历史计算存储容器
        // 访问对象 0阶数值序列
        // 访问周期 前1个计算周期
        y = A.value(Calculus::ValueType::X, 1);

        // 访问积分器历史计算存储容器
        // 访问对象 1阶数值序列
        // 访问周期 前1个计算周期
        y = A.value(Calculus::ValueType::XDot, 1);
    }
}

// 3.3 多维线性插值计算案例
void Tutorial::Example33(void)
{
    cout << "------ 3.3 多维线性插值计算案例 ------" << endl
         << endl;

    // 通过数组定义插值数据序列
    // 插值数轴X alpha = [0, 2]
    // 插值数轴X beta = [-2, -1, 0, 1, 2]
    // 插值数轴Y gamma = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
    // 函数关系 gamma = f(alpha, beta)
    double alpha[2] = {0, 2};
    double beta[5] = {-2, -1, 0, 1, 2};
    double gamma[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    double value;
    bool ok;

    // 使用静态公有函数直接进行插值计算
    value = LookupTable::interp(2, 5, alpha, beta, gamma, 2, 0, false);

    // 创建数据插值表对象
    LookupTable A, B;

    // 使用预设数组设置线性插值表对象数据内容
    A.setX(2, alpha);
    A.setX(5, beta);
    A.setY(10, gamma);

    // 检查线性插值表数据合规性
    ok = A.isVaild();
    cout << "LookupTable A" << endl;
    cout << A << endl;

    // 将线性插值表对象存储为标准格式的本地数据文件
    A.save("Example33.txt");

    // 读取外部数据文件以初始化线性插值表对象
    ok = B.load("Example33.txt");
    cout << "LookupTable B" << endl;
    cout << B << endl;

    // 使用线性插值表的公有函数进行线性插值计算
    value = B.interp(0, 1, false);
}

// 3.4 线性系统分析案例
void Tutorial::Example34(void)
{
    cout << "------ 3.4 线性系统分析案例 ------" << endl
         << endl;

    // 数据记录文件
    ofstream stream;

    // 参考系统结构框图
    // u -->x--> D --> A --> G --.--> y
    //     -^                |
    //      |------ H -------|

    // 创建系统环节传递函数
    Polynomial num, den;
    TransferFunction D, A, G, H;

    num = Polynomial{100.0};
    den = Polynomial{0.0, 5.0, 1.0};
    G = TransferFunction(num, den);

    num = Polynomial{1.0};
    den = Polynomial{1.0, 0.005};
    A = TransferFunction(num, den);

    num = Polynomial{1.0, 0.2};
    den = Polynomial{1.0, 0.02};
    D = TransferFunction(num, den);

    num = Polynomial{1.0};
    den = Polynomial{1.0};
    H = TransferFunction(num, den);

    // 计算系统开环传递函数
    TransferFunction G_OPEN;

    G_OPEN = D * A * G;

    cout << "开环传递函数 Open-Loop" << endl
         << endl;
    cout << G_OPEN << endl;

    // 计算开环传递函数的零极点
    TransferFunction::ZeroPoleInfo S_ZPK;

    S_ZPK = TransferFunction::zeroPole(G_OPEN);

    cout << "系统零点 Zero" << endl;

    for (int n = 0; n < S_ZPK.zero.size(); n++)
    {
        Basic::display(cout, S_ZPK.zero[n]);
        cout << endl;
    }

    cout << endl;

    cout << "系统极点 Pole" << endl;

    for (int n = 0; n < S_ZPK.pole.size(); n++)
    {
        Basic::display(cout, S_ZPK.pole[n]);
        cout << endl;
    }

    cout << endl;

    cout << "系统增益 K" << endl;
    cout << S_ZPK.k << endl;
    cout << endl;

    // 计算开环传递函数的对数频率特性
    vector<double> omega, phase, gain;

    // 计算自定义频率序列 [0.01, 0.1, 1, 10, 100] rad/s 的对数频率特性
    omega = vector<double>{0.01, 0.1, 1, 10, 100};
    TransferFunction::bode(G_OPEN, omega, gain, phase);

    // 默认计算 0.01~100 rad/s 频率区间的对数频率特性
    omega.clear();
    TransferFunction::bode(G_OPEN, omega, gain, phase);

    // 记录仿真结果
    stream.open("Example34A.txt");
    stream << "Omega(rad/s)\tGain(dB)\tPhase(deg)" << endl;

    for (int n = 0; n < omega.size(); n++)
    {
        stream << omega[n] << "\t" << gain[n] << "\t" << phase[n] << endl;
    }

    stream.close();

    // 计算开环传递函数的幅值/相位裕度
    TransferFunction::MarginInfo S_MARGIN;

    // 计算幅值/相位裕度
    S_MARGIN = TransferFunction::margin(omega, gain, phase);

    cout << "稳定裕度指标 Margin" << endl;
    cout << "Gm(dB)\t" << S_MARGIN.Gm << endl;
    cout << "Pm(deg)\t" << S_MARGIN.Pm << endl;
    cout << "Wcg(rad/s)\t" << S_MARGIN.Wcg << endl;
    cout << "Wcp(rad/s)\t" << S_MARGIN.Wcp << endl;
    cout << endl;

    // 计算系统闭环传递函数
    TransferFunction G_CLOSE;

    G_CLOSE = TransferFunction::feedback(D * A * G, H);

    cout << "闭环传递函数 Closed-Loop" << endl
         << endl;
    cout << G_CLOSE << endl;

    // 计算闭环传递函数的阶跃响应
    vector<double> time, value;

    // 计算自定义时间区间 0:0.01:5s 的单位阶跃响应
    TransferFunction::step(G_CLOSE, time, value, 0.01, 5.0);

    // 默认计算 0:0.005:10s 时间区间的单位阶跃响应
    TransferFunction::step(G_CLOSE, time, value);

    // 记录仿真结果
    stream.open("Example34B.txt");
    stream << "Time(s)\tValue(-)" << endl;

    for (int n = 0; n < time.size(); n++)
    {
        stream << time[n] << "\t" << value[n] << endl;
    }

    stream.close();

    // 计算系统的时域性能指标
    TransferFunction::StepInfo S_STEP;

    S_STEP = TransferFunction::step(time, value);

    cout << "动态性能指标 StepInfo" << endl;
    cout << "RiseTime(s)\t" << S_STEP.riseTime << endl;
    cout << "PeakTime(s)\t" << S_STEP.peakTime << endl;
    cout << "SettingTime(s)\t" << S_STEP.settingTime << endl;
    cout << "PeakValue(-)\t" << S_STEP.peakValue << endl;
    cout << "FinalValue(-)\t" << S_STEP.finalValue << endl;
    cout << "OverShoot(-)\t" << S_STEP.overShoot << endl;
    cout << endl;

    // 计算闭环传递函数的离散化形式 T = 0.005 s
    TransferFunction G_DISC;

    G_DISC = TransferFunction::discrete(G_CLOSE, 0.005);

    cout << "离散传递函数 Discrete" << endl
         << endl;
    cout << G_DISC << endl;

    // 记录仿真结果
    stream.open("Example34C.txt");
    stream << "Time(s)\tValue(-)" << endl;

    // 模拟仿真计算周期循环
    for (int n = 0; n < 2000; n++)
    {
        double t, u, y;

        // 更新仿真时间 t(n+1)=t(n)+0.005
        t = n * 0.005;

        // 模拟单位阶跃输入
        u = 1.0;

        // 计算离散传递函数的时序输出
        y = G_DISC.response(u);

        // 打印时序计算结果
        stream << t << "\t" << y << endl;
    }

    stream.close();
}

// 3.5 坐标系算法案例
void Tutorial::Example35(void)
{
    cout << "------ 3.5 坐标系算法案例 ------" << endl
         << endl;

    // 计算坐标系旋转矩阵
    // 绕X轴旋转 30 deg
    Matrix A;

    A = Coordinate::rotationX(30.0);

    // 计算坐标系投影矩阵
    // 旋转关系 导航系 > 机体系
    // 滚动角 0.0 deg  航向角 0.0 deg 俯仰角 30.0 deg
    A = Coordinate::cNav2Body(0.0, 0.0, 30.0);

    // 计算导航系速度向量在机体系投影 [10.0, 0.0, 0.0] m/s
    Vector vN(3), vB(3);

    vN(AXIS_N) = 10.0;
    vN(AXIS_U) = 0.0;
    vN(AXIS_E) = 0.0;

    vB = A * vN;

    // 计算欧拉角 [gamma, psi, zeta] deg
    // 坐标轴旋转顺序 Y-Z-X
    Vector eular(3);

    eular = Coordinate::eular(A, Coordinate::RotationType::YZX);

    // 计算四元数 [q0, q1, q2, q3]
    Vector Q(4);

    Q = Coordinate::quaternion(A);

    // 计算攻角/侧滑角 deg
    double alpha, beta;

    alpha = Coordinate::alpha(vB(AXIS_X), vB(AXIS_Y), vB(AXIS_Z));
    beta = Coordinate::beta(vB(AXIS_X), vB(AXIS_Y), vB(AXIS_Z));

    // 计算弹道倾角/弹道偏角 deg
    double theta, psiV;

    theta = Coordinate::theta(vN(AXIS_N), vN(AXIS_U), vN(AXIS_E));
    psiV = Coordinate::psiV(vN(AXIS_N), vN(AXIS_U), vN(AXIS_E));
}

// 3.6 地球几何算法案例
void Tutorial::Example36(void)
{
    cout << "------ 3.6 地球几何算法案例 ------" << endl
         << endl;

    // 计算指定纬度 deg 位置的曲率半径 m
    double Rm, Rn;

    Rm = Geodesy::radiusM(39.0);
    Rn = Geodesy::radiusN(39.0);

    // 计算指定纬度 deg 和 海拔高度 m 位置的重力加速度 m/s^2
    double g;

    g = Geodesy::gravity(39.0, 0.0);

    // 计算 A 至 B 的 大地距离 m 和 方位角 deg
    double S, A;
    double longitudeA = 122.0, latitudeA = 39.0;
    double longitudeB = 122.05, latitudeB = 39.05;

    Geodesy::vincentSA(longitudeA, latitudeA, longitudeB, latitudeB, S, A);

    // 换算 地心地固系坐标 ECEF 与 经纬高 LLA
    Vector ECEF(3), LLA(3);

    LLA(AXIS_LONGITUDE) = 122.0;
    LLA(AXIS_LATITUDE) = 39.0;
    LLA(AXIS_ALTITUDE) = 0.0;

    ECEF = Geodesy::cLLA2ECEF(LLA(AXIS_LONGITUDE), LLA(AXIS_LATITUDE), LLA(AXIS_ALTITUDE));
    LLA = Geodesy::cECEF2LLA(ECEF(AXIS_X), ECEF(AXIS_Y), ECEF(AXIS_Z));
}

// 5.7 经典控制器设计案例
void Tutorial::Example57(void)
{
    cout << "------ 5.7 经典控制器设计案例 ------" << endl
         << endl;

    // 数据记录文件
    ofstream stream;

    // 控制参数
    double Kp = 12.0, Ki = 1.0, Kd = 5.0;

    // 创建环节传递函数
    Polynomial num, den;
    TransferFunction G, PID;

    num = Polynomial{1.0};
    den = Polynomial{0.0, 5.0, 6.0, 1.0};
    G = TransferFunction(num, den);

    num = Polynomial{Ki, Kp, Kd};
    den = Polynomial{0.0, 1.0};
    PID = TransferFunction(num, den);

    // 计算系统闭环传递函数
    TransferFunction Gc;

    Gc = TransferFunction::feedback(PID * G, 1.0);

    cout << "闭环传递函数 Gc" << endl
         << endl;
    cout << Gc << endl;

    // 计算闭环传递函数的离散化形式 采样时间 0.005 s
    TransferFunction Gz;

    Gz = TransferFunction::discrete(Gc, 0.005);

    cout << "离散传递函数 Gz" << endl
         << endl;
    cout << Gz << endl;

    // 记录仿真结果
    stream.open("Example57.txt");
    stream << "Time(s)\tU(-)\tY(-)" << endl;

    // 模拟仿真计算周期循环 0 ~ 10s
    for (int n = 0; n < 2000; n++)
    {
        double t, u, y;

        // 更新仿真时间 t(n+1)=t(n)+0.005
        t = n * 0.005;

        // 模拟单位阶跃输入
        u = 1.0;

        // 计算离散传递函数的时序输出
        y = Gz.response(u);

        // 打印时序计算结果
        stream << t << "\t" << u << "\t" << y << endl;
    }

    stream.close();
    cout << endl;
}

// 5.8 控制系统时域分析案例
void Tutorial::Example58(void)
{
    cout << "------ 5.8 控制系统时域分析案例 ------" << endl
         << endl;

    // 数据记录文件
    ofstream stream;

    // 待研究系统参数
    double Kp = 2.0;
    double Ta = 0.1;

    // 创建分子/分母多项式
    Polynomial num, den;

    num = Polynomial{Kp};
    den = Polynomial{Kp, 1.0, Ta};

    // 创建系统闭环传递函数
    TransferFunction Gc;

    Gc = TransferFunction(num, den, TransferFunction::SystemType::Continuous);

    cout << "闭环传递函数 Gc" << endl
         << endl;
    cout << Gc << endl;

    // 计算闭环传递函数的单位阶跃响应
    vector<double> time, value;

    // 自定义时域区间 0:0.005:5s
    TransferFunction::step(Gc, time, value, 0.005, 5.0);

    // 记录仿真结果
    stream.open("Example58.txt");
    stream << "Time(s)\tValue(-)" << endl;

    for (int n = 0; n < time.size(); n++)
    {
        stream << time[n] << "\t" << value[n] << endl;
    }

    stream.close();

    // 计算系统的时域性能指标
    TransferFunction::StepInfo ST;

    ST = TransferFunction::step(time, value);

    cout << "动态性能指标 Step" << endl;
    cout << "RiseTime(s)\t" << ST.riseTime << endl;
    cout << "PeakTime(s)\t" << ST.peakTime << endl;
    cout << "SettingTime(s)\t" << ST.settingTime << endl;
    cout << "PeakValue(-)\t" << ST.peakValue << endl;
    cout << "FinalValue(-)\t" << ST.finalValue << endl;
    cout << "OverShoot(-)\t" << ST.overShoot << endl;
    cout << endl;
}

// 5.10 控制系统频域分析案例
void Tutorial::Example510(void)
{
    cout << "------ 5.10 控制系统频域分析案例 ------" << endl
         << endl;

    // 数据记录文件
    ofstream stream;

    // 待研究系统参数
    double Kp = 2.0;
    double Ta = 0.1;

    // 创建分子/分母多项式
    Polynomial num, den;

    num = Polynomial{Kp};
    den = Polynomial{1.0, Ta};

    // 创建系统开环传递函数
    TransferFunction Go;

    Go = TransferFunction(num, den, TransferFunction::SystemType::Continuous);

    cout << "开环传递函数 Go" << endl
         << endl;
    cout << Go << endl;

    // 记录仿真结果
    stream.open("Example510A.txt");
    stream << "Omega(rad/s)\tReal(-)\tImag(-)\tGain(dB)\tPhase(deg)" << endl;

    // 计算指定频率区间幅相特性 0.01~1000 rad/s
    vector<double> omega, L;

    L = Basic::linearSpace(0.01, 0.099, 98);
    omega.insert(omega.end(), L.begin(), L.end());
    L = Basic::linearSpace(0.1, 0.99, 98);
    omega.insert(omega.end(), L.begin(), L.end());
    L = Basic::linearSpace(1.0, 9.8, 89);
    omega.insert(omega.end(), L.begin(), L.end());
    L = Basic::linearSpace(10.0, 98.0, 89);
    omega.insert(omega.end(), L.begin(), L.end());
    L = Basic::linearSpace(100.0, 990.0, 89);
    omega.insert(omega.end(), L.begin(), L.end());
    L = Basic::linearSpace(1000.0, 9900.0, 89);
    omega.insert(omega.end(), L.begin(), L.end());

    for (int n = 0; n < omega.size(); n++)
    {
        double w, a, p, gain, phase;

        // 计算指定频率对应的 幅值 - / 相角 deg
        w = omega[n];
        Go.response(w, a, p);

        // 单位制换算 幅值 dB / 相位 deg
        gain = 20 * log10(a);
        phase = p;

        stream << w << "\t" << a * cos(p / RAD_TO_DEG) << "\t" << a * sin(p / RAD_TO_DEG) << "\t" << gain << "\t" << phase << endl;
    }

    stream.close();

    // 计算开环传递函数的对数频率特性
    vector<double> gain, phase;

    // 默认计算频率区间 0.01~100 rad/s
    omega.clear();
    TransferFunction::bode(Go, omega, gain, phase);

    // 记录仿真结果
    stream.open("Example510B.txt");
    stream << "Omega(rad/s)\tGain(dB)\tPhase(deg)" << endl;

    for (int n = 0; n < omega.size(); n++)
    {
        stream << omega[n] << "\t" << gain[n] << "\t" << phase[n] << endl;
    }

    stream.close();

    // 计算开环传递函数的幅值/相位裕度
    TransferFunction::MarginInfo SF;

    // 计算幅值/相位裕度
    SF = TransferFunction::margin(omega, gain, phase);

    cout << "稳定裕度指标 Margin" << endl;
    cout << "Gm(dB)\t" << SF.Gm << endl;
    cout << "Pm(deg)\t" << SF.Pm << endl;
    cout << "Wcg(rad/s)\t" << SF.Wcg << endl;
    cout << "Wcp(rad/s)\t" << SF.Wcp << endl;
    cout << endl;
}

// 5.15 经典控制回路设计案例
void Tutorial::Example515(void)
{
    cout << "------ 5.15 经典姿态控制回路设计案例 ------" << endl
         << endl;

    // 数据记录文件
    ofstream stream;

    // 动力学系数
    double a22 = 0.0, a24 = 25.49, a25 = 6.98, a34 = 0.07, a35 = 0.0;

    // 环节参数
    double Ta = 0.01, Xa = 0.7, Ts = 0.003, Xs = 0.7;

    // 控制参数
    double Kp = 39.0, Ki = 0.0, Kd = 3.0;

    // 创建环节传递函数
    Polynomial num, den;
    TransferFunction Ga, Gw, Gs;

    num = Polynomial{a25 * a34 - a24 * a35, a25};
    den = Polynomial{a24 + a22 * a34, a22 + a34, 1.0};
    Gw = TransferFunction(num, den);

    num = Polynomial{1.0};
    den = Polynomial{1.0, 2 * Xa * Ta, Ta * Ta};
    Ga = TransferFunction(num, den);

    num = Polynomial{1.0};
    den = Polynomial{1.0, 2 * Xs * Ts, Ts * Ts};
    Gs = TransferFunction(num, den);

    // 计算等效前向通路/反馈支路
    TransferFunction G, H;

    G = Ga * Gw * TransferFunction(0.0, 1.0, 0.0) * Kp;
    H = (TransferFunction(0.0, 0.0, 1.0) * Gs * (Kd / Kp)) + 1.0;

    // 计算系统开环传递函数
    TransferFunction Go;

    Go = G * H;
    Go.miniReal();

    cout << "开环传递函数 Go" << endl
         << endl;
    cout << Go << endl;

    // 计算开环传递函数的对数频率特性
    vector<double> omega, gain, phase;

    // 默认计算频率区间 0.01~100 rad/s
    omega.clear();
    TransferFunction::bode(Go, omega, gain, phase);

    // 记录仿真结果
    stream.open("Example515A.txt");
    stream << "Omega(rad/s)\tGain(dB)\tPhase(deg)" << endl;

    for (int n = 0; n < omega.size(); n++)
    {
        stream << omega[n] << "\t" << gain[n] << "\t" << phase[n] << endl;
    }

    stream.close();

    // 计算开环传递函数的幅值/相位裕度
    TransferFunction::MarginInfo SF;

    // 计算幅值/相位裕度
    SF = TransferFunction::margin(omega, gain, phase);

    cout << "稳定裕度指标 Margin" << endl;
    cout << "Gm(dB)\t" << SF.Gm << endl;
    cout << "Pm(deg)\t" << SF.Pm << endl;
    cout << "Wcg(rad/s)\t" << SF.Wcg << endl;
    cout << "Wcp(rad/s)\t" << SF.Wcp << endl;
    cout << endl;

    // 计算系统开环传递函数
    TransferFunction Gc;

    Gc = TransferFunction::feedback(G, H);
    Gc.miniReal();

    cout << "闭环传递函数 Gc" << endl
         << endl;
    cout << Gc << endl;

    // 计算闭环系统的单位阶跃响应 0~10s
    vector<double> time, value;

    TransferFunction::step(Gc, time, value);

    // 记录仿真结果
    stream.open("Example515B.txt");
    stream << "Time(s)\tValue(-)" << endl;

    for (int n = 0; n < time.size(); n++)
    {
        stream << time[n] << "\t" << value[n] << endl;
    }

    stream.close();

    // 计算系统的时域性能指标
    TransferFunction::StepInfo ST;

    ST = TransferFunction::step(time, value);

    cout << "动态性能指标 Step" << endl;
    cout << "RiseTime(s)\t" << ST.riseTime << endl;
    cout << "PeakTime(s)\t" << ST.peakTime << endl;
    cout << "SettingTime(s)\t" << ST.settingTime << endl;
    cout << "PeakValue(-)\t" << ST.peakValue << endl;
    cout << "FinalValue(-)\t" << ST.finalValue << endl;
    cout << "OverShoot(-)\t" << ST.overShoot << endl;
    cout << endl;
}

// 7 无人飞行器仿真应用案例
void Tutorial::Example7(void)
{
    cout << "------ 7 无人飞行器仿真应用案例 ------" << endl
         << endl;

    // 自定义仿真工程
    UAVSimulation simulation;

    // 仿真准备流程
    simulation.setup();

    // 仿真初始化流程
    simulation.initialize();

    // 仿真单步运行流程
    simulation.step();
}

// 构造函数
UAVMass::UAVMass()
{
    // 初始化输入量
    this->input.fuelUsed = 0;

    // 初始化输出量
    this->output.mass = 0;
    this->output.CG.init(3);
    this->output.Inertia.init(3, 3);

    // 初始化输参数量
    this->parameter.mass = 200.0;

    this->parameter.CG.init(3);
    this->parameter.CG(AXIS_X) = 1.05;
    this->parameter.CG(AXIS_Y) = 0;
    this->parameter.CG(AXIS_Z) = 0;

    this->parameter.Inertia.init(3, 3);
    this->parameter.Inertia(AXIS_X, AXIS_X) = 8.56;
    this->parameter.Inertia(AXIS_Y, AXIS_Y) = 91.26;
    this->parameter.Inertia(AXIS_Z, AXIS_Z) = 91.26;
}

// 析构函数
UAVMass::~UAVMass()
{
}

// 初始化函数
void UAVMass::initialize(void)
{
    // 调用单步计算函数刷新输出量
    this->step();
}

// 单步运行函数
void UAVMass::step(void)
{
    // 此处默认飞行器质量参数为常值
    // 亦可补充质量参数得动态变化规律
    this->output.mass = this->parameter.mass;
    this->output.CG = this->parameter.CG;
    this->output.Inertia = this->parameter.Inertia;
}

// 构造函数
UAVAeroDynamic::UAVAeroDynamic()
{
    // 初始化输入量
    this->input.dynamicPressure = 0;
    this->input.velocity = 0;
    this->input.mach = 0;
    this->input.alpha = 0;
    this->input.beta = 0;
    this->input.omega.init(3);
    this->input.delta.init(3);
    this->input.CG.init(3);

    // 初始化输出量
    this->output.Cxyz.init(3);
    this->output.Mxyz.init(3);
    this->output.force.init(3);
    this->output.torque.init(3);

    // 初始化参数量
    this->parameter.L = 0.76;
    this->parameter.S = 0.05;

    this->parameter.CR.init(3);
    this->parameter.CR(AXIS_X) = 1.37;
    this->parameter.CR(AXIS_Y) = 0;
    this->parameter.CR(AXIS_Z) = 0;

    // 初始化临时量
    this->cV2B.init(3, 3);
    this->vCxyz.init(3);
    this->vMxyz.init(3);

    // 气动数据列表
    this->data.vMach[0] = 0.50;
    this->data.vMach[1] = 0.75;
    this->data.vMach[2] = 1.00;
    this->data.vMach[3] = 1.25;
    this->data.vMach[4] = 1.50;

    this->data.vCxMach[0] = -0.7728;
    this->data.vCxMach[1] = -0.7529;
    this->data.vCxMach[2] = -0.7328;
    this->data.vCxMach[3] = -0.7017;
    this->data.vCxMach[4] = -0.6590;

    this->data.vCyMach[0] = 0.3552;
    this->data.vCyMach[1] = 0.3335;
    this->data.vCyMach[2] = 0.3238;
    this->data.vCyMach[3] = 0.3013;
    this->data.vCyMach[4] = 0.2780;

    this->data.vCyAlpha[0] = 0.4134;
    this->data.vCyAlpha[1] = 0.3523;
    this->data.vCyAlpha[2] = 0.3094;
    this->data.vCyAlpha[3] = 0.2843;
    this->data.vCyAlpha[4] = 0.2972;

    this->data.vCzBeta[0] = -0.3237;
    this->data.vCzBeta[1] = -0.2454;
    this->data.vCzBeta[2] = -0.1996;
    this->data.vCzBeta[3] = -0.1710;
    this->data.vCzBeta[4] = -0.1618;

    this->data.vMxBeta[0] = 0.0058;
    this->data.vMxBeta[1] = 0.0011;
    this->data.vMxBeta[2] = 0.0000;
    this->data.vMxBeta[3] = 0.0000;
    this->data.vMxBeta[4] = 0.0000;

    this->data.vMxDeltaX[0] = -0.0531;
    this->data.vMxDeltaX[1] = -0.0431;
    this->data.vMxDeltaX[2] = -0.0354;
    this->data.vMxDeltaX[3] = -0.0307;
    this->data.vMxDeltaX[4] = -0.0302;

    this->data.vMyBeta[0] = -0.0519;
    this->data.vMyBeta[1] = -0.0470;
    this->data.vMyBeta[2] = -0.0442;
    this->data.vMyBeta[3] = -0.0444;
    this->data.vMyBeta[4] = -0.0421;

    this->data.vMyDeltaY[0] = -0.3237;
    this->data.vMyDeltaY[1] = -0.2454;
    this->data.vMyDeltaY[2] = -0.1996;
    this->data.vMyDeltaY[3] = -0.1710;
    this->data.vMyDeltaY[4] = -0.1618;

    this->data.vMzAlpha[0] = -0.0391;
    this->data.vMzAlpha[1] = -0.0355;
    this->data.vMzAlpha[2] = -0.0323;
    this->data.vMzAlpha[3] = -0.0312;
    this->data.vMzAlpha[4] = -0.0305;

    this->data.vMzDeltaZ[0] = -0.2689;
    this->data.vMzDeltaZ[1] = -0.2069;
    this->data.vMzDeltaZ[2] = -0.1758;
    this->data.vMzDeltaZ[3] = -0.1551;
    this->data.vMzDeltaZ[4] = -0.1413;
}

// 析构函数
UAVAeroDynamic::~UAVAeroDynamic()
{
}

// 初始化函数
void UAVAeroDynamic::initialize(void)
{
    // 初始化气动数据插值表对象
    this->vCxMach.setX(5, this->data.vMach);
    this->vCxMach.setY(5, this->data.vCxMach);

    this->vCyMach.setX(5, this->data.vMach);
    this->vCyMach.setY(5, this->data.vCyMach);
    this->vCyAlpha.setX(5, this->data.vMach);
    this->vCyAlpha.setY(5, this->data.vCyAlpha);

    this->vCzBeta.setX(5, this->data.vMach);
    this->vCzBeta.setY(5, this->data.vCzBeta);

    this->vMxBeta.setX(5, this->data.vMach);
    this->vMxBeta.setY(5, this->data.vMxBeta);
    this->vMxDeltaX.setX(5, this->data.vMach);
    this->vMxDeltaX.setY(5, this->data.vMxDeltaX);

    this->vMyBeta.setX(5, this->data.vMach);
    this->vMyBeta.setY(5, this->data.vMyBeta);
    this->vMyDeltaY.setX(5, this->data.vMach);
    this->vMyDeltaY.setY(5, this->data.vMyDeltaY);

    this->vMzAlpha.setX(5, this->data.vMach);
    this->vMzAlpha.setY(5, this->data.vMzAlpha);
    this->vMzDeltaZ.setX(5, this->data.vMach);
    this->vMzDeltaZ.setY(5, this->data.vMzDeltaZ);

    // 初始化输出量
    this->output.Cxyz(0) = 0.0;
    this->output.Cxyz(1) = 0.0;
    this->output.Cxyz(2) = 0.0;

    this->output.Mxyz(0) = 0.0;
    this->output.Mxyz(1) = 0.0;
    this->output.Mxyz(2) = 0.0;

    this->output.force(0) = 0.0;
    this->output.force(1) = 0.0;
    this->output.force(2) = 0.0;

    this->output.torque(0) = 0.0;
    this->output.torque(1) = 0.0;
    this->output.torque(2) = 0.0;
}

// 单步运行函数
void UAVAeroDynamic::step(void)
{
    Vector r(3), cb(3), mb(3);
    double v;

    // 初始化临时量
    this->vCxyz(0) = 0.0;
    this->vCxyz(1) = 0.0;
    this->vCxyz(2) = 0.0;

    this->vMxyz(0) = 0.0;
    this->vMxyz(1) = 0.0;
    this->vMxyz(2) = 0.0;

    // 计算阻力系数
    v = this->vCxMach.interp(this->input.mach);
    this->vCxyz(AXIS_X) += v;

    // 计算升力系数
    v = this->vCyMach.interp(this->input.mach);
    this->vCxyz(AXIS_Y) += v;

    v = this->vCyAlpha.interp(this->input.mach);
    this->vCxyz(AXIS_Y) += v * this->input.alpha;

    // 计算侧向力系数
    v = this->vCzBeta.interp(this->input.mach);
    this->vCxyz(AXIS_Z) += v * this->input.beta;

    // 计算滚动力矩系数
    v = this->vMxBeta.interp(this->input.mach);
    this->vMxyz(AXIS_X) += v * this->input.beta;

    v = this->vMxDeltaX.interp(this->input.mach);
    this->vMxyz(AXIS_X) += v * this->input.delta(AXIS_X);

    // 计算航向力矩系数
    v = this->vMyBeta.interp(this->input.mach);
    this->vMxyz(AXIS_Y) += v * this->input.beta;

    v = this->vMyDeltaY.interp(this->input.mach);
    this->vMxyz(AXIS_Y) += v * this->input.delta(AXIS_Y);

    // 计算俯仰力矩系数
    v = this->vMzAlpha.interp(this->input.mach);
    this->vMxyz(AXIS_Z) += v * this->input.alpha;

    v = this->vMzDeltaZ.interp(this->input.mach);
    this->vMxyz(AXIS_Z) += v * this->input.delta(AXIS_Z);

    // 计算坐标系投影矩阵 速度系>机体系
    this->cV2B = Coordinate::cVec2Body(input.alpha, input.beta);

    // 计算实际重心与参考重心相对位置关系
    r = MathOperation::relativePosition(parameter.CR, input.CG);

    // 将速度系气动力系数投影至机体系
    cb = this->cV2B * this->vCxyz;

    // 计算气动力系数对于力矩系数得影响偏量
    mb = Vector::cross(r, cb);

    // 计算综合气动力系数
    this->output.Cxyz(AXIS_X) = this->vCxyz(AXIS_X);
    this->output.Cxyz(AXIS_Y) = this->vCxyz(AXIS_Y);
    this->output.Cxyz(AXIS_Z) = this->vCxyz(AXIS_Z);

    // 计算综合气动力矩系数
    this->output.Mxyz(AXIS_X) = this->vMxyz(AXIS_X) + mb(AXIS_X) / parameter.L;
    this->output.Mxyz(AXIS_Y) = this->vMxyz(AXIS_Y) + mb(AXIS_Y) / parameter.L;
    this->output.Mxyz(AXIS_Z) = this->vMxyz(AXIS_Z) + mb(AXIS_Z) / parameter.L;

    // 计算综合气动力 N
    this->output.force(AXIS_X) = cb(AXIS_X) * this->input.dynamicPressure * parameter.S;
    this->output.force(AXIS_Y) = cb(AXIS_Y) * this->input.dynamicPressure * parameter.S;
    this->output.force(AXIS_Z) = cb(AXIS_Z) * this->input.dynamicPressure * parameter.S;

    // 计算综合气动力矩 N*m
    this->output.torque(AXIS_X) = this->output.Mxyz(AXIS_X) * this->input.dynamicPressure * parameter.S * parameter.L;
    this->output.torque(AXIS_Y) = this->output.Mxyz(AXIS_Y) * this->input.dynamicPressure * parameter.S * parameter.L;
    this->output.torque(AXIS_Z) = this->output.Mxyz(AXIS_Z) * this->input.dynamicPressure * parameter.S * parameter.L;
}

// 构造函数
UAVPropulsor::UAVPropulsor()
{
    // 初始化输入量
    this->input.time = 0;
    this->input.CG.init(3);

    // 初始化输出量
    this->output.thrust = 0;
    this->output.fuelUsed = 0;
    this->output.force.init(3);
    this->output.torque.init(3);

    // 初始化参数量
    this->parameter.location.init(3);
    this->parameter.location(AXIS_X) = 2.17;
    this->parameter.location(AXIS_Y) = 0;
    this->parameter.location(AXIS_Z) = 0;

    // 初始化中间量
    this->vStartTime = 0;

    // 初始化动力性能数据
    this->data.vTime[0] = 0;
    this->data.vTime[1] = 5;
    this->data.vTime[2] = 10;
    this->data.vTime[3] = 20;
    this->data.vTime[4] = 50;
    this->data.vTime[5] = 70;
    this->data.vTime[6] = 80;
    this->data.vTime[7] = 85;
    this->data.vTime[8] = 90;
    this->data.vTime[9] = 100;

    this->data.vThrust[0] = 0;
    this->data.vThrust[1] = 1500;
    this->data.vThrust[2] = 4500;
    this->data.vThrust[3] = 5000;
    this->data.vThrust[4] = 5000;
    this->data.vThrust[5] = 4500;
    this->data.vThrust[6] = 3500;
    this->data.vThrust[7] = 2500;
    this->data.vThrust[8] = 1000;
    this->data.vThrust[9] = 0;
}

// 析构函数
UAVPropulsor::~UAVPropulsor()
{
}

// 初始化函数
void UAVPropulsor::initialize(void)
{
    // 初始化动力数据插值表
    this->vThrust.setX(10, this->data.vTime);
    this->vThrust.setY(10, this->data.vThrust);

    // 初始化输出量
    this->output.thrust = 0;
    this->output.fuelUsed = 0;

    this->output.force(0) = 0;
    this->output.force(1) = 0;
    this->output.force(2) = 0;

    this->output.torque(0) = 0;
    this->output.torque(1) = 0;
    this->output.torque(2) = 0;

    // 初始化中间量
    this->vStartTime = 0;
}

// 单步运行函数
void UAVPropulsor::step(void)
{
    double time;
    ASL::Vector r(3);

    // 计算当前工作时刻 s
    time = this->input.time - this->vStartTime;

    // 计算额定推力 N
    this->output.thrust = this->vThrust.interp(time);

    // 计算燃料消耗 kg
    this->output.fuelUsed = 0;

    // 计算综合推力 N 机体系
    this->output.force(AXIS_X) = this->output.thrust;
    this->output.force(AXIS_Y) = 0;
    this->output.force(AXIS_Z) = 0;

    // 计算发动机安装位置与飞行器重心得相对位置关系
    r = MathOperation::relativePosition(this->parameter.location, this->input.CG);

    // 计算综合推力矩 N*m 机体系
    this->output.torque = Vector::cross(r, this->output.force);
}

// 构造函数
UAVTarget::UAVTarget()
{
    // 初始化输入量
    this->input.time = 0;

    // 初始化输出量
    this->output.LLA.init(3);
    this->output.vNav.init(3);

    // 初始化参数量
    this->parameter.LLA.init(3);
    this->parameter.LLA(AXIS_LONGITUDE) = 122.0;
    this->parameter.LLA(AXIS_LATITUDE) = 39.0;
    this->parameter.LLA(AXIS_ALTITUDE) = 100.0;

    this->parameter.vNav.init(3);
    this->parameter.vNav(AXIS_N) = 0;
    this->parameter.vNav(AXIS_U) = 0;
    this->parameter.vNav(AXIS_E) = 20.0;

    // 初始化中间量
    this->vStartTime = 0;
}

// 析构函数
UAVTarget::~UAVTarget()
{
}

// 初始化函数
void UAVTarget::initialize(void)
{
    // 初始化中间量
    this->vStartTime = 0;

    // 默认目标以初始位置为起点作匀速直线运动
    this->output.LLA = this->parameter.LLA;
    this->output.vNav = this->parameter.vNav;
}

// 单步运行函数
void UAVTarget::step(void)
{
    double time;
    double lamda, phi, height;
    double v, psiV, A, S;

    // 计算目标运动时间 s
    time = this->input.time - this->vStartTime;

    // 计算目标弹道偏角 -180~+180 deg
    psiV = Coordinate::psiV(this->output.vNav(AXIS_X), this->output.vNav(AXIS_Y), this->output.vNav(AXIS_Z));

    // 换算为大地方位角 0~360 deg
    if (psiV >= 0.0)
    {
        A = 360.0 - psiV;
    }
    else
    {
        A = 0.0 - psiV;
    }

    // 计算目标当前经纬度 deg
    v = sqrt(this->parameter.vNav(AXIS_N) * this->parameter.vNav(AXIS_N) + this->parameter.vNav(AXIS_E) * this->parameter.vNav(AXIS_E));
    S = v * time;
    Geodesy::vincentLB(this->parameter.LLA(AXIS_LONGITUDE), this->parameter.LLA(AXIS_LATITUDE), S, A, lamda, phi);

    // 计算目标当前海拔高度 m
    v = this->output.vNav(AXIS_U);
    height = this->parameter.LLA(AXIS_ALTITUDE) + v * time;

    // 更新目标实时位置
    this->output.LLA(AXIS_LONGITUDE) = lamda;
    this->output.LLA(AXIS_LATITUDE) = phi;
    this->output.LLA(AXIS_ALTITUDE) = height;

    // 更新目标实时速度
    this->output.vNav(AXIS_N) = this->parameter.vNav(AXIS_N);
    this->output.vNav(AXIS_U) = this->parameter.vNav(AXIS_U);
    this->output.vNav(AXIS_E) = this->parameter.vNav(AXIS_E);
}

// 构造函数
UAVGuidance::UAVGuidance()
{
    // 初始化输入量
    this->input.aircraftLLA.init(3);
    this->input.aircraftVelocity.init(3);
    this->input.targetLLA.init(3);
    this->input.targetVelocity.init(3);

    // 初始化输出量
    this->output.relativeDistance = 0;
    this->output.ApproachingSpeed = 0;
    this->output.LOSAngle.init(3);
    this->output.LOSRate.init(3);

    // 初始化中间量
    this->cN2L.init(3, 3);
}

// 析构函数
UAVGuidance::~UAVGuidance()
{
}

// 初始化函数
void UAVGuidance::initialize(void)
{
    this->output.relativeDistance = 0;
    this->output.ApproachingSpeed = 0;
    this->output.LOSAngle(0) = 0;
    this->output.LOSAngle(1) = 0;
    this->output.LOSAngle(2) = 0;
    this->output.LOSRate(0) = 0;
    this->output.LOSRate(1) = 0;
    this->output.LOSRate(2) = 0;
}

// 单步运行函数
void UAVGuidance::step(void)
{
    double S, A;
    double x, y, z;
    Vector v(3);

    // 计算飞行器与目标的大地距离和方位角
    Geodesy::vincentSA(this->input.aircraftLLA(AXIS_LONGITUDE), this->input.aircraftLLA(AXIS_LATITUDE), this->input.targetLLA(AXIS_LONGITUDE), this->input.targetLLA(AXIS_LATITUDE), S, A);

    // 计算目标相对于飞行器在导航系内的坐标值
    x = S * cos(A / RAD_TO_DEG) - 0;
    y = this->input.targetLLA(AXIS_ALTITUDE) - this->input.aircraftLLA(AXIS_ALTITUDE);
    z = S * sin(A / RAD_TO_DEG) - 0;

    // 计算相对距离 m
    this->output.relativeDistance = sqrt(x * x + y * y + z * z);

    // 计算空间视线角 deg
    this->output.LOSAngle(AXIS_X) = 0;
    this->output.LOSAngle(AXIS_Y) = -RAD_TO_DEG * atan2(z, x);
    this->output.LOSAngle(AXIS_Z) = RAD_TO_DEG * asin(y / this->output.relativeDistance);

    // 计算坐标系投影矩阵 导航系>视线系
    this->cN2L = Coordinate::rotationZ(this->output.LOSAngle(AXIS_Z)) * Coordinate::rotationY(this->output.LOSAngle(AXIS_Y));
    v = this->cN2L * (this->input.aircraftVelocity - this->input.targetVelocity);

    // 计算接近速度 m/s
    this->output.ApproachingSpeed = -v(AXIS_X);

    // 计算空间视线角速度 deg/s
    this->output.LOSRate(AXIS_X) = 0;
    this->output.LOSRate(AXIS_Y) = RAD_TO_DEG * v(AXIS_Z) / (this->output.relativeDistance * cos(this->output.LOSAngle(AXIS_Z) / RAD_TO_DEG));
    this->output.LOSRate(AXIS_Z) = -RAD_TO_DEG * v(AXIS_Y) / this->output.relativeDistance;
}

// 构造函数
UAVControlSystem::UAVControlSystem()
{
    // 初始化输入量
    this->input.time = 0;
    this->input.LLA.init(3);
    this->input.vNav.init(3);
    this->input.eular.init(3);
    this->input.omega.init(3);
    this->input.LOSRate.init(3);

    // 初始化输出量
    this->output.delta.init(3);
    this->output.eularCommand.init(3);
    this->output.accelerationCommand.init(3);

    // 初始化参数量
    this->parameter.cruiseAltitude = 1000.0;

    this->parameter.targetLocation.init(3);
    this->parameter.targetLocation(AXIS_LONGITUDE) = 122.0;
    this->parameter.targetLocation(AXIS_LATITUDE) = 39.0;
    this->parameter.targetLocation(AXIS_ALTITUDE) = 100.0;

    this->parameter.launchEular.init(3);
    this->parameter.launchEular(AXIS_X) = 0;
    this->parameter.launchEular(AXIS_Y) = 0;
    this->parameter.launchEular(AXIS_Z) = 0;

    // 初始化中间量
    this->vLaunchTime = 0;
    this->vCruiseTime = 0;
    this->vAttackTime = 0;
    this->vMode = 0;
    this->vLaunchEular.init(3);
    this->vCruiseEular.init(3);
    this->vAttackEular.init(3);
    this->vLaunchLLA.init(3);
    this->vCruiseLLA.init(3);
    this->vAttackLLA.init(3);
    this->cN2B.init(3, 3);
    this->cN2T.init(3, 3);
}

// 析构函数
UAVControlSystem::~UAVControlSystem()
{
}

// 初始化函数
void UAVControlSystem::initialize(double stepSize)
{
    // 默认初始状态为零控制量
    this->output.delta(AXIS_X) = 0;
    this->output.delta(AXIS_Y) = 0;
    this->output.delta(AXIS_Z) = 0;

    this->output.eularCommand(AXIS_X) = 0;
    this->output.eularCommand(AXIS_Y) = 0;
    this->output.eularCommand(AXIS_Z) = 0;

    this->output.accelerationCommand(AXIS_X) = 0;
    this->output.accelerationCommand(AXIS_Y) = 0;
    this->output.accelerationCommand(AXIS_Z) = 0;

    this->output.altitudeCommand = 0;

    // 默认初始时刻的飞行任务模式为发射模式
    this->vLaunchTime = 0;
    this->vCruiseTime = 0;
    this->vAttackTime = 0;
    this->vMode = UAVControlSystem::Launch;
    this->vLaunchEular = this->parameter.launchEular;
    this->cDeltaHeight.initialize(Calculus::Integrate, 0.0, stepSize, Calculus::Euler);
}

// 单步运行函数
void UAVControlSystem::step(void)
{
    // 计算飞行器与预设目标区域的大地距离和方位角
    Geodesy::vincentSA(this->input.LLA(AXIS_LONGITUDE), this->input.LLA(AXIS_LATITUDE), this->parameter.targetLocation(AXIS_LONGITUDE), this->parameter.targetLocation(AXIS_LATITUDE), this->distance, this->angle);

    // 换算为目标航向角 -180~+180 deg
    if (this->angle >= 0.0 && this->angle < 180.0)
    {
        this->angle = 0.0 - this->angle;
    }
    else
    {
        this->angle = 360.0 - this->angle;
    }

    if (this->input.time >= 55.0)
    {
        this->vMode = this->vMode;
    }

    // 当飞行时间小于30s时启用发射模式
    if (this->input.time < 20.0)
    {
        this->vMode = UAVControlSystem::Launch;
        this->vCruiseTime = this->input.time;
        this->vCruiseLLA = this->input.LLA;
        this->vCruiseEular = this->input.eular;
        this->launch();
    }
    // 当飞行器与预设目标区域距离 R>8km 时启用巡航模式
    else if (this->distance >= 8000.0)
    {
        this->vMode = UAVControlSystem::Cruise;
        this->vAttackTime = this->input.time;
        this->vAttackLLA = this->input.LLA;
        this->vAttackEular = this->input.eular;
        this->vAttackAcceleration = this->input.aBody;
        this->cruise();
    }
    // 当飞行器进入预设目标区域范围时启用攻击模式
    else
    {
        this->vMode = UAVControlSystem::Attack;
        this->attack();
    }
}

// 发射模式控制算法
void UAVControlSystem::launch(void)
{
    double v, z, t;

    // 滚动通道采用PD控制回路 让滚动角维持于零位
    v = 0.15 * (0 - this->input.eular(AXIS_X)) + 0.05 * (0 - this->input.omega(AXIS_X));
    this->output.eularCommand(AXIS_X) = 0;
    this->output.delta(AXIS_X) = Basic::limit(-v, -15, +15);

    // 航向通道采用PD控制回路 让航向角维持于初始朝向
    v = 0.0 * (this->vLaunchEular(AXIS_Y) - this->input.eular(AXIS_Y)) + 0.08 * (0 - this->input.omega(AXIS_Y));
    this->output.eularCommand(AXIS_Y) = this->vLaunchEular(AXIS_Y);
    this->output.delta(AXIS_Y) = Basic::limit(-v, -15, +15);

    // 人为设置俯仰角过渡函数作为参考基准
    t = this->input.time - this->vLaunchTime;
    z = 2.0 + (this->vLaunchEular(AXIS_Z) - 2.0) * exp(-t / 5.0);
    this->output.eularCommand(AXIS_Z) = z;

    // 俯仰通道采用PD控制回路 让俯仰角按预设变化规律过渡至平稳状态
    v = 0.35 * (z - this->input.eular(AXIS_Z)) + 0.15 * (0 - this->input.omega(AXIS_Z));
    this->output.delta(AXIS_Z) = Basic::limit(-v, -15, +15);
}

// 巡航模式控制算法
void UAVControlSystem::cruise(void)
{
    double a, b, c;
    double v, z, t;

    // 滚动通道采用PD控制回路 让滚动角维持于零位
    v = 0.15 * (0 - this->input.eular(AXIS_X)) + 0.05 * (0 - this->input.omega(AXIS_X));
    this->output.eularCommand(AXIS_X) = 0;
    this->output.delta(AXIS_X) = Basic::limit(-v, -15, +15);

    // 航向通道采用PD控制回路 让航向角指向预设目标区域方向
    t = this->input.time - this->vCruiseTime;
    z = this->angle + (this->vCruiseEular(AXIS_Y) - this->angle) * exp(-t / 10.0);
    v = 0.20 * (z - this->input.eular(AXIS_Y)) + 0.08 * (0 - this->input.omega(AXIS_Y));
    this->output.eularCommand(AXIS_Y) = z;
    this->output.delta(AXIS_Y) = Basic::limit(-v, -15, +15);

    // 俯仰通道采用PD控制回路 让飞行器维持于升重平衡的状态
    t = this->input.time - this->vCruiseTime;
    z = 2.0 + (this->vCruiseEular(AXIS_Z) - 2.0) * exp(-t / 5.0);
    this->output.eularCommand(AXIS_Z) = z;
    a = 0.35 * (z - this->input.eular(AXIS_Z)) + 0.15 * (0 - this->input.omega(AXIS_Z));
    // a = Basic::limit(-a, -10, +10);

    // 高度通道采用PID控制回路 让飞行器稳定飞行于预设高度
    t = this->input.time - this->vCruiseTime;
    z = this->parameter.cruiseAltitude + (this->vCruiseLLA(AXIS_ALTITUDE) - this->parameter.cruiseAltitude) * exp(-t / 10.0);
    this->output.altitudeCommand = z;
    v = z - this->input.LLA(AXIS_ALTITUDE);
    b = 0.1 * v + 0.002 * this->cDeltaHeight.step(v) + 0.08 * (0.0 - this->input.vNav(AXIS_U));
    // b = Basic::limit(-b, -10, +10);

    // 综合俯仰角与高度控制指令
    this->output.delta(AXIS_Z) = Basic::limit(-a - b, -15, +15);
}

// 攻击模式控制算法
void UAVControlSystem::attack(void)
{
    double v, a, b, c, t, z;
    double theta, psiV;
    Vector AT(3), AB(3);

    // 滚动通道采用PD控制回路 让滚动角维持于零位
    v = 0.10 * (0 - this->input.eular(AXIS_X)) + 0.05 * (0 - this->input.omega(AXIS_X));
    this->output.eularCommand(AXIS_X) = 0;
    this->output.accelerationCommand(AXIS_X) = 0;
    this->output.delta(AXIS_X) = Basic::limit(-v, -15, +15);

    // 计算弹道倾角与弹道偏角 deg
    theta = Coordinate::theta(this->input.vNav(AXIS_X), this->input.vNav(AXIS_Y), this->input.vNav(AXIS_Z));
    psiV = Coordinate::psiV(this->input.vNav(AXIS_X), this->input.vNav(AXIS_Y), this->input.vNav(AXIS_Z));

    // 计算坐标系投影矩阵
    this->cN2T = Coordinate::cNav2Traj(theta, psiV);
    this->cN2B = Coordinate::cNav2Body(this->input.eular(AXIS_X), this->input.eular(AXIS_Y), this->input.eular(AXIS_Z));

    // 由比例导引法计算弹道系加速度指令 m/s^2
    v = Vector::norm(this->input.vNav);
    AT(AXIS_X) = 0;
    AT(AXIS_Y) = 3.0 * v * this->input.LOSRate(AXIS_Z) / RAD_TO_DEG;
    AT(AXIS_Z) = -3.0 * v * cos(theta / RAD_TO_DEG) * this->input.LOSRate(AXIS_Y) / RAD_TO_DEG;

    // 将弹道系加速度指令投影至机体系 m/s^2
    AB = this->cN2B * Matrix::transpose(this->cN2T) * AT;

    // 航向通道采用PD控制回路 沿理侧向想追踪轨迹运动
    t = this->input.time - this->vAttackTime;
    z = AB(AXIS_Z) + (this->vAttackAcceleration(AXIS_Z) - AB(AXIS_Z)) * exp(-t / 1.0);
    this->output.eularCommand(AXIS_Y) = 0;
    this->output.accelerationCommand(AXIS_Z) = z;

    a = 0.08 * (z - this->input.aBody(AXIS_Z));
    b = 0.20 * (0 - this->input.omega(AXIS_Y));
    v = a - b;
    this->output.delta(AXIS_Y) = Basic::limit(v, -15, +15);

    // 俯仰通道采用PD控制回路 沿纵向理想追踪轨迹运动
    t = this->input.time - this->vAttackTime;
    z = AB(AXIS_Y) + (this->vAttackAcceleration(AXIS_Y) - AB(AXIS_Y)) * exp(-t / 5.0);
    this->output.eularCommand(AXIS_Z) = 0;
    this->output.accelerationCommand(AXIS_Y) = z;
    this->output.altitudeCommand = 0;

    a = 0.01 * (z - this->input.aBody(AXIS_Y));
    b = 0.10 * (0 - this->input.omega(AXIS_Z));
    v = -a - b;
    this->output.delta(AXIS_Z) = Basic::limit(v, -15, +15);
}

// 构造函数
UAVActuator::UAVActuator()
{
    input.u = 0;
    output.y = 0;

    parameter.initialValue = 0;
    parameter.k = 1.0;
    parameter.locationRange[0] = 0;
    parameter.locationRange[1] = 0;
    parameter.velocityRange[0] = 0;
    parameter.velocityRange[1] = 0;
}

// 析构函数
UAVActuator::~UAVActuator()
{
}

// 初始化函数
bool UAVActuator::initialize(double stepSize)
{
    cIntegrator.initialize(Calculus::ModeType::Integrate, parameter.initialValue, stepSize, Calculus::MethodType::Euler);
    output.y = parameter.initialValue;

    return true;
}

// 单步运行函数
bool UAVActuator::step(void)
{
    double u, v;

    u = Basic::limit(input.u, parameter.locationRange[0], parameter.locationRange[1]);
    v = (u - output.y) * parameter.k;
    v = Basic::limit(v, parameter.velocityRange[0], parameter.velocityRange[1]);
    cIntegrator.step(v);
    output.y = cIntegrator.value(Calculus::ValueType::X, 0);

    return true;
}

// 构造函数
UAVSimulation::UAVSimulation()
{
    startTime = 0;
    stopTime = 0;
    stepSize = 0;
    currentTime = 0;
    currentCount = 0;
}

// 析构函数
UAVSimulation::~UAVSimulation()
{
}

// 准备函数
void UAVSimulation::setup(void)
{
    cout << "正在准备仿真试验 setup" << endl;

    // 仿真求解配置
    startTime = 0.0;
    stopTime = 100.0;
    stepSize = 0.005;
    currentTime = 0.0;
    currentCount = 0;

    // 六自由度运动学模型参数
    cDOFModel.parameter.LLA(0) = 122.0;
    cDOFModel.parameter.LLA(1) = 39.0;
    cDOFModel.parameter.LLA(2) = 1500.0;

    cDOFModel.parameter.vBody(0) = 200.0;
    cDOFModel.parameter.vBody(1) = 0;
    cDOFModel.parameter.vBody(2) = 0;

    cDOFModel.parameter.eular(0) = 0;
    cDOFModel.parameter.eular(1) = 0;
    cDOFModel.parameter.eular(2) = 0;

    cDOFModel.parameter.omega(0) = 0;
    cDOFModel.parameter.omega(1) = 0;
    cDOFModel.parameter.omega(2) = 0;

    // 副翼执行机构参数
    cActutorModelX.parameter.k = 1.0 / 0.05;
    cActutorModelX.parameter.locationRange[0] = -10.0;
    cActutorModelX.parameter.locationRange[1] = +10.0;
    cActutorModelX.parameter.velocityRange[0] = -80.0;
    cActutorModelX.parameter.velocityRange[1] = +80.0;

    // 方向舵执行机构参数
    cActutorModelY.parameter.k = 1.0 / 0.08;
    cActutorModelY.parameter.locationRange[0] = -15.0;
    cActutorModelY.parameter.locationRange[1] = +15.0;
    cActutorModelY.parameter.velocityRange[0] = -60.0;
    cActutorModelY.parameter.velocityRange[1] = +60.0;

    // 升降舵执行机构参数
    cActutorModelZ.parameter.k = 1.0 / 0.08;
    cActutorModelZ.parameter.locationRange[0] = -15.0;
    cActutorModelZ.parameter.locationRange[1] = +15.0;
    cActutorModelZ.parameter.velocityRange[0] = -60.0;
    cActutorModelZ.parameter.velocityRange[1] = +60.0;

    // 目标运动参数
    cTargetModel.parameter.LLA(0) = 122.02;
    cTargetModel.parameter.LLA(1) = 39.35;
    cTargetModel.parameter.LLA(2) = 0.0;

    cTargetModel.parameter.vNav(0) = 0.0;
    cTargetModel.parameter.vNav(1) = 0.0;
    cTargetModel.parameter.vNav(2) = 10.0;

    // 飞行控制参数
    cControlModel.parameter.cruiseAltitude = 1000.0;
    cControlModel.parameter.launchEular = cDOFModel.parameter.eular;
    cControlModel.parameter.targetLocation = cTargetModel.parameter.LLA;

    // 创建本地数据文件
    cStream.open("Example7.txt");
    cStream.setf(ios::showpos);
    cStream.setf(ios::fixed);
    cStream.precision(8);

    // 仿真数据标题文本
    cStream << "Time(s)\t";

    cStream << "Longitude(deg)\tLatitude(deg)\tAltitude(m)\t";
    cStream << "V-North(m/s)\tV-Up(m/s)\tV-East(m/s)\t";
    cStream << "Accel-X(m/s^2)\tAccel-Y(m/s^2)\tAccel-Z(m/s^2)\t";

    cStream << "Gamma(deg)\tPsi(deg)\tZeta(deg)\t";
    cStream << "Omega-X(deg/s)\tOmega-Y(deg/s)\tOmega-Z(deg/s)\t";

    cStream << "Mach(-)\tAlpha(deg)\tBeta(deg)\t";
    cStream << "Velocity(m/s)\tTheta(deg)\tPsiV(deg)\t";

    cStream << "C-Delta-X(deg)\tC-Delta-Y(deg)\tC-Delta-Z(deg)\t";
    cStream << "C-Gamma(deg)\tC-Psi(deg)\tC-Zeta(deg)\t";
    cStream << "C-Accel-X(m/s^2)\tC-Accel-Y(m/s^2)\tC-Accel-Z(m/s^2)\t";
    cStream << "C-Location-X(m)\tC-Location-Y(m)\tC-Location-Z(m)\t";

    cStream << "Time(s)\tDistance(m)\tSpeed(m/s)\t";
    cStream << "LOS-Angel-X(deg)\tLOS-Angel-Y(deg)\tLOS-Angel-Z(deg)\t";
    cStream << "LOS-Rate-X(deg/s)\tLOS-Rate-Y(deg/s)\tLOS-Rate-Z(deg/s)\t";

    cStream << "Aircraft-X(m)\tAircraft-Y(m)\tAircraft-Z(m)\t";
    cStream << "Target-X(m)\tTarget-Y(m)\tTarget-Z(m)\t";

    cStream << "Propulsor-X(N)\tPropulsor-Y(N)\tPropulsor-Z(N)\t";
    cStream << "Propulsor-X(N*m)\tPropulsor-Y(N*m)\tPropulsor-Z(N*m)\t";
    cStream << "AeroDynamic-X(N)\tAeroDynamic-Y(N)\tAeroDynamic-Z(N)\t";
    cStream << "AeroDynamic-X(N*m)\tAeroDynamic-Y(N*m)\tAeroDynamic-Z(N*m)\t";

    cStream << endl;
}

// 初始化函数
void UAVSimulation::initialize(void)
{
    cout << "正在初始化仿真条件 initialize" << endl;

    // 仿真时间记录
    currentTime = 0.0;
    currentCount = 0;

    // 系列模型初始化
    cDOFModel.initialize(stepSize);

    cAtmosModel.input.altitude = cDOFModel.parameter.LLA(2);
    cAtmosModel.initialize();

    cMassModel.input.fuelUsed = 0.0;
    cMassModel.initialize();

    cAeroModel.initialize();
    cControlModel.initialize(stepSize);

    cThrustModel.initialize();

    cActutorModelX.initialize(stepSize);
    cActutorModelY.initialize(stepSize);
    cActutorModelZ.initialize(stepSize);

    cTargetModel.initialize();
    cGuidanceModel.initialize();
}

// 仿真运行函数
void UAVSimulation::step(void)
{
    double v = 0.0, mach = 0.0, q, t;
    double alpha = 0.0, beta = 0.0, theta = 0.0, psi = 0.0;
    Vector RA(3), RT(3);
    double L, B, S, A;

    cout << "正在执行仿真解算 step" << endl;

    L = cDOFModel.parameter.LLA(AXIS_LONGITUDE);
    B = cDOFModel.parameter.LLA(AXIS_LATITUDE);

    while (currentTime < stopTime)
    {
        // 更新当前仿真时间
        currentTime = currentCount * stepSize;
        currentCount++;

        // 计算飞行器与目标关于起点位置的方位坐标
        Geodesy::vincentSA(L, B, cDOFModel.output.LLA(AXIS_LONGITUDE), cDOFModel.output.LLA(AXIS_LATITUDE), S, A);

        RA(AXIS_X) = S * cos(A / RAD_TO_DEG);
        RA(AXIS_Y) = cDOFModel.output.LLA(AXIS_ALTITUDE);
        RA(AXIS_Z) = S * sin(A / RAD_TO_DEG);

        Geodesy::vincentSA(L, B, cTargetModel.output.LLA(AXIS_LONGITUDE), cTargetModel.output.LLA(AXIS_LATITUDE), S, A);

        RT(AXIS_X) = S * cos(A / RAD_TO_DEG);
        RT(AXIS_Y) = cTargetModel.output.LLA(AXIS_ALTITUDE);
        RT(AXIS_Z) = S * sin(A / RAD_TO_DEG);

        // 记录仿真数据
        cStream << currentTime << "\t";
        cStream << cDOFModel.output.LLA << cDOFModel.output.vNav << cDOFModel.output.aBody;
        cStream << cDOFModel.output.eular << cDOFModel.output.omega;
        cStream << mach << "\t" << alpha << "\t" << beta << "\t";
        cStream << v << "\t" << theta << "\t" << psi << "\t";

        cStream << cControlModel.output.delta << cControlModel.output.eularCommand;
        cStream << cControlModel.output.accelerationCommand;
        cStream << 0.0 << "\t" << cControlModel.output.altitudeCommand << "\t" << 0.0 << "\t";

        cStream << currentTime << "\t" << cGuidanceModel.output.relativeDistance << "\t" << cGuidanceModel.output.ApproachingSpeed << "\t";
        cStream << cGuidanceModel.output.LOSAngle << cGuidanceModel.output.LOSRate;
        cStream << RA << RT;
        cStream << cThrustModel.output.force << cThrustModel.output.torque;
        cStream << cAeroModel.output.force << cAeroModel.output.torque;

        cStream << endl;

        // 仿真终止条件I 海拔高度小于零
        if (cDOFModel.output.LLA(AXIS_ALTITUDE) <= 0.0)
        {
            break;
        }
        // 仿真终止条件II 相对距离小于预定值
        else if (currentTime >= 10.0 && cGuidanceModel.output.relativeDistance <= 1.0)
        {
            break;
        }

        // 飞行运动学模型
        cDOFModel.input.force = cAeroModel.output.force + cThrustModel.output.force;
        cDOFModel.input.torque = cAeroModel.output.torque + cThrustModel.output.torque;
        cDOFModel.input.mass = cMassModel.output.mass;
        cDOFModel.input.inertia = cMassModel.output.Inertia;
        cDOFModel.input.g = Geodesy::gravity(cDOFModel.output.LLA(1), cDOFModel.output.LLA(2));
        cDOFModel.step();

        // 大气环境模型
        cAtmosModel.input.altitude = cDOFModel.output.LLA(2);
        cAtmosModel.step();

        // 飞行参数解算
        theta = Coordinate::theta(cDOFModel.output.vNav(AXIS_X), cDOFModel.output.vNav(AXIS_Y), cDOFModel.output.vNav(AXIS_Z));
        psi = Coordinate::psiV(cDOFModel.output.vNav(AXIS_X), cDOFModel.output.vNav(AXIS_Y), cDOFModel.output.vNav(AXIS_Z));
        alpha = Coordinate::alpha(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        beta = Coordinate::beta(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        v = Vector::norm(cDOFModel.output.vBody);
        t = Geodesy::temperature(cDOFModel.output.LLA(2));
        mach = v / Geodesy::acousticVelocity(t);
        q = 0.5 * cAtmosModel.output.density * v * v;

        // 质量特性模型
        cMassModel.input.fuelUsed = 0;
        cMassModel.step();

        // 气动特性模型
        cAeroModel.input.dynamicPressure = q;
        cAeroModel.input.velocity = v;
        cAeroModel.input.mach = mach;
        cAeroModel.input.alpha = alpha;
        cAeroModel.input.beta = beta;
        cAeroModel.input.delta(0) = cActutorModelX.output.y;
        cAeroModel.input.delta(1) = cActutorModelY.output.y;
        cAeroModel.input.delta(2) = cActutorModelZ.output.y;
        cAeroModel.input.omega = cDOFModel.output.omega;
        cAeroModel.input.CG = cMassModel.output.CG;
        cAeroModel.step();

        // 动力系统模型
        cThrustModel.input.time = currentTime;
        cThrustModel.input.CG = cMassModel.output.CG;
        cThrustModel.step();

        // 控制系统模型
        cControlModel.input.time = currentTime;
        cControlModel.input.LLA = cDOFModel.output.LLA;
        cControlModel.input.vNav = cDOFModel.output.vNav;
        cControlModel.input.aBody = cDOFModel.output.aBody;
        cControlModel.input.eular = cDOFModel.output.eular;
        cControlModel.input.omega = cDOFModel.output.omega;
        cControlModel.input.LOSRate = cGuidanceModel.output.LOSRate;
        cControlModel.step();

        // 执行机构模型
        cActutorModelX.input.u = cControlModel.output.delta(0);
        cActutorModelX.step();
        cActutorModelY.input.u = cControlModel.output.delta(1);
        cActutorModelY.step();
        cActutorModelZ.input.u = cControlModel.output.delta(2);
        cActutorModelZ.step();

        // 目标模型
        cTargetModel.input.time = currentTime;
        cTargetModel.step();

        // 相对运动关系模型
        cGuidanceModel.input.aircraftLLA = cDOFModel.output.LLA;
        cGuidanceModel.input.aircraftVelocity = cDOFModel.output.vNav;
        cGuidanceModel.input.targetLLA = cTargetModel.output.LLA;
        cGuidanceModel.input.targetVelocity = cTargetModel.output.vNav;
        cGuidanceModel.step();
    }

    cStream.close();
    cout << "已完成仿真试验流程 terminate" << endl;
}